/*
 * BeagleStates.cpp
 *
 *  Created on: Apr 3, 2013
 *      Author: root
 */

#include <opencv/highgui.h>
#include "BeagleStates.h"
#include "Funcs.h"


void ScanDocks::Execute(StateMachine* s,Buffer* m_Buffer,GlobalVars* v,CvCapture* cap){
		std::cout << "first loop"<<std::endl;
		//scanning Blocks;
		//m_Buffer->ScanSea();
		//v->Sea = AutoRangeDistance(cap);


		std::cout << "second loop"<<std::endl;
		//m_Buffer->ScanRail();
		//v->Rail = AutoRangeDistance(cap);
		//m_Buffer->RestPos();
		m_Buffer->OutWhole();
		m_Buffer->writeBuffer("l 39");
		m_Buffer->checkFinish();
		m_Buffer->writeBuffer("q 90");
		m_Buffer->checkFinish();
		s->changeState(ScanAndPlaceBlocks::instance());
}

void ScanAndPlaceBlocks::Execute(StateMachine* s,Buffer* m_Buffer,GlobalVars* v,CvCapture* cap){
		std::cout << "third loop"<<std::endl;
		int posdX = 0;
		m_Buffer->writeBuffer("g",v->BlockCords[0][0],v->BlockCords[0][1]);
		m_Buffer->checkFinish();
		v->setPos(v->BlockCords[0][0],v->BlockCords[0][1]);
		m_Buffer->writeBuffer("h 0");
		m_Buffer->checkFinish();
		m_Buffer->writeBuffer("b 140");
		m_Buffer->checkFinish();

		for(int x=0;x<14;x++){
			m_Buffer->writeBuffer("t 90");
			m_Buffer->checkFinish();
			if(x != 0){
				posdX = v->BlockCords[x][0] - v->posX;
				v->setPos(v->BlockCords[x][0]);
				if(posdX > 0){
					m_Buffer->writeBuffer("f",posdX);
				}
				if(posdX < 0){
					posdX = -posdX;
					m_Buffer->writeBuffer("b",posdX);
				}
			}
			m_Buffer->ScanBlockFunc();
			usleep(100);
			v->Blocks[x]  = AutoRange(cap);
			if(v->Blocks[x][1] == 2){
				posdX = v->RailLoadingZone[0] - v->posX;
				v->setPos(v->RailLoadingZone[0]);
				v->TurretDegree = 270;
			}
			else if(v->Blocks[x][1] == 1){
				posdX = v->SeaLoadingZone[0] - v->posX;
				v->setPos(v->SeaLoadingZone[0]);
				v->TurretDegree = 0;
			}
			else
				continue;
			while(!AutoAlignChasey(cap,v->Blocks[x][0],m_Buffer,1,v->posX));
			m_Buffer->PickUpBlock();
			if(posdX > 0)
				m_Buffer->writeBuffer("f",posdX);
			if(posdX < 0){
				posdX = -posdX;
				m_Buffer->writeBuffer("b",posdX);
			}
			m_Buffer->writeBuffer("t", v->TurretDegree);
			m_Buffer->ScanDock();
			while(!AutoAlignTurret(cap,v->Blocks[x][0],m_Buffer,v->TurretDegree));
			m_Buffer->PlaceBlock();
		}
	s->changeState(Terminate::instance());
}

void PlaceBlocks::Execute(StateMachine* s,Buffer* m_Buffer,GlobalVars* v,CvCapture* cap){

	std::cout << "fourth loop"<<std::endl;
	for(int x=0;x<6;x++){
		int firstBlock = 0;


		for(int x=0;x<14;x++){
			if(v->Blocks[x][1] == 1){
				firstBlock = x;
				v->Blocks[x][1] =-1;
				break;
			}
		}
		int posdX = v->BlockCords[firstBlock][0] - v->posX;
		v->setPos(v->BlockCords[firstBlock][0]);
		if(posdX > 0){
			m_Buffer->writeBuffer("f",posdX);
		}
		if(posdX < 0){
			posdX = -posdX;
			m_Buffer->writeBuffer("b",posdX);
		}
		
		v->TurretDegree = 90;
		m_Buffer->writeBuffer("t", v->TurretDegree);
		while(!AutoAlignChasey(cap,v->BlockCords[firstBlock][0],m_Buffer,0,v->posX));
		m_Buffer->PickUpBlock();
		m_Buffer->checkFinish();

		int firstZone = 0;


		for(int x=0;x<6;x++){
			if(v->Sea[x] == v->Blocks[firstBlock][0]) {
				firstZone = x;
				break;
			}
		}

		posdX = v->SeaLoadingZone[0] - v->posX;
		if(posdX > 0)
			m_Buffer->writeBuffer("f",posdX);
		if(posdX < 0){
			posdX = -posdX;
			m_Buffer->writeBuffer("b",posdX);
		}
		v->setPos(v->SeaLoadingZone[0]);
		v->TurretDegree = 180;
		m_Buffer->writeBuffer("t", v->TurretDegree);
		m_Buffer->writeBuffer("a 4 800");
		while(!AutoAlignTurret(cap,v->Blocks[firstBlock][0],m_Buffer,v->TurretDegree));
		m_Buffer->PlaceBlock();

	}


	/**********************************************************************/
	std::cout << "fifth loop"<<std::endl;
	for(int x=0;x<6;x++){
			int firstBlock = 0;

			for(int x=0;x<14;x++){
				if(v->Blocks[x][1] == 2){
					firstBlock = x;
					v->Blocks[x][1] =-1;
					break;
				}
			}

			int posdX = v->BlockCords[firstBlock][0] - v->posX;
				if(posdX >= 0)
					m_Buffer->writeBuffer("f",posdX);
				if(posdX < 0){
					posdX = -posdX;
					m_Buffer->writeBuffer("b",posdX);
				}
			v->setPos(v->BlockCords[firstBlock][0]);
			m_Buffer->checkFinish();
			
			v->TurretDegree = 90;
			m_Buffer->writeBuffer("t TurretDegree");
			while(!AutoAlignChasey(cap,v->BlockCords[firstBlock][0],m_Buffer,0,v->posX));
			m_Buffer->PickUpBlock();

			int firstZone = 0;

			for(int x=0;x<6;x++){
				if(v->Rail[x] == v->Blocks[firstBlock][0]){
					firstZone = x;
					break;
				}
			}


			posdX = v->RailLoadingZone[0] - v->posX;
			if(posdX > 0)
				m_Buffer->writeBuffer("f",posdX);
			if(posdX < 0){
				posdX = -posdX;
				m_Buffer->writeBuffer("b",posdX);
			}
			v->setPos(v->RailLoadingZone[0]);
			v->TurretDegree = 270;
			m_Buffer->writeBuffer("t", v->TurretDegree);
			while(!AutoAlignTurret(cap,v->Blocks[firstBlock][0],m_Buffer,v->TurretDegree));
			m_Buffer->PlaceBlock();
		}

	s->changeState(Terminate::instance());
}

void PlaceBlocksAir::Execute(StateMachine* s,Buffer* m_Buffer,GlobalVars* v,CvCapture* cap){
	std::cout << "sixth loop";
			int firstBlock = 0;
			int secondBlock = 0;
			for(int x=0;x<14;x++){
				if(v->Blocks[x][1] == 0){
					firstBlock = x;
					v->Blocks[x][1] =-1;
					break;
				}
			}
			for(int x=0;x<14;x++){
				if(v->Blocks[x][1] == 0){
					secondBlock = x;
					v->Blocks[x][1] =-1;
					break;
				}
			}

			int posdX = v->BlockCords[firstBlock][0] - v->posX;
				if(posdX >= 0)
					m_Buffer->writeBuffer("f",posdX);
				if(posdX < 0){
					posdX+= posdX*2;
					m_Buffer->writeBuffer("b",posdX);
			}
			v->setPos(v->BlockCords[firstBlock][0]);
			m_Buffer->checkFinish();
			m_Buffer->writeBuffer("y 5");
			m_Buffer->checkFinish();

			posdX = v->BlockCords[secondBlock][0] - v->posX;
				if(posdX >= 0)
					m_Buffer->writeBuffer("f",posdX);
				if(posdX < 0){
					posdX+= posdX*2;
					m_Buffer->writeBuffer("b",posdX);
				}
			v->setPos(v->BlockCords[secondBlock][0]);
			m_Buffer->checkFinish();
			m_Buffer->writeBuffer("y 1");
			m_Buffer->checkFinish();




			m_Buffer->goUpRamp();
			m_Buffer->writeBuffer("y 7");
			m_Buffer->checkFinish();

			v->Air = AutoRangeDistanceAir(cap);

			if(v->Air[0] == v->Blocks[secondBlock][0]){
				m_Buffer->writeBuffer("y 2");
				m_Buffer->checkFinish();

				m_Buffer->writeBuffer("y 8");
				m_Buffer->checkFinish();
			}
			else{
				m_Buffer->writeBuffer("y 1");
				m_Buffer->checkFinish();

				m_Buffer->writeBuffer("y 8");
				m_Buffer->checkFinish();


			}

			s->changeState(Terminate::instance());

}

void Terminate::Execute(StateMachine* s,Buffer* m_Buffer,GlobalVars* v,CvCapture* cap){
	s->StopRun();
}


void Debug::Execute(StateMachine* s,Buffer* m_Buffer,GlobalVars* v,CvCapture* cap){
	if(v->debug){
		using namespace std;
		Buffer* m_Buffer = new Buffer();
		unsigned char* buff = new unsigned char[1024];
		string  command;
		while(1){
			cout << "enter command" << std::endl;
			getline(cin,command);
			if(command == "exit")
				break;
			m_Buffer->writeBuffer(command.c_str());
			command = "";
			buff = m_Buffer->updateInput();
			cout << buff << endl;

		}
	}
	if(!v->debug){/*
	if(!v->debug1){
			v->debug1 = true;
			m_Buffer->ScanBlockFunc();
		}*/
		AutoRange(cap);

	}
}
